#include "MatrixHelper.h"
#include "Common.h"
#include "MathHelper.h"

MatrixHelper::MatrixHelper()
{
}

MatrixHelper::~MatrixHelper()
{
}

void MatrixHelper::LoadIdentity(Matrix matrix)
{
	memset(matrix, 0, sizeof(Matrix));
	matrix[MATRIX_SCALE_X_INDEX] = 1;
	matrix[MATRIX_SCALE_X_INDEX] = 1;
	matrix[MATRIX_SCALE_X_INDEX] = 1;
	matrix[15] = 1;
}

void MatrixHelper::LoadRotation(const Quaternion& rotation)
{
	Matrix matrix;

	float d = (rotation.X * rotation.X) + (rotation.Y) * (rotation.Y) + (rotation.Z * rotation.Z) + (rotation.W * rotation.W);
    float s = 2.0f / d;
	float xs = rotation.X * s,   ys = rotation.Y * s,   zs = rotation.Z * s;
	float wx = rotation.W * xs,  wy = rotation.W * ys,  wz = rotation.W * zs;
    float xx = rotation.X * xs,  xy = rotation.X * ys,  xz = rotation.X * zs;
	float yy = rotation.Y * ys,  yz = rotation.Y * zs,  zz = rotation.Z * zs;
   
    matrix[MATRIX_RIGHT_X_INDEX] = 1.0f - (yy + zz);
    matrix[MATRIX_RIGHT_Y_INDEX] = xy - wz;
    matrix[MATRIX_RIGHT_Z_INDEX] = xz + wy;

    matrix[MATRIX_UP_X_INDEX] = xy + wz;
    matrix[MATRIX_UP_Y_INDEX] = 1.0f - (xx + zz);
    matrix[MATRIX_UP_Z_INDEX] = yz - wx;

    matrix[MATRIX_FORWARD_X_INDEX] = xz - wy;
    matrix[MATRIX_FORWARD_Y_INDEX] = yz + wx;
    matrix[MATRIX_FORWARD_Z_INDEX] = 1.0f - (xx + yy);

	glLoadMatrixf(matrix);
}

void MatrixHelper::MultiplyRotation(const Quaternion& rotation)
{
	Vector3f axis;
	float angle = 0.0f;

	rotation.ToAxisAngle(axis, angle);

	angle = RadiansToDegrees(angle);

	glRotatef(angle, axis.X, axis.Y, axis.Z);
}

void MatrixHelper::GetUp(const Matrix matrix, Vector3f& up)
{
	up.X = matrix[MATRIX_UP_X_INDEX];
	up.Y = matrix[MATRIX_UP_Y_INDEX];
	up.Z = matrix[MATRIX_UP_Z_INDEX];
}

void MatrixHelper::GetRight(const Matrix matrix, Vector3f& right)
{
	right.X = matrix[MATRIX_RIGHT_X_INDEX];
	right.Y = matrix[MATRIX_RIGHT_Y_INDEX];
	right.Z = matrix[MATRIX_RIGHT_Z_INDEX];
}

void MatrixHelper::GetForward(const Matrix matrix, Vector3f& forward)
{
	forward.X = matrix[MATRIX_FORWARD_X_INDEX];
	forward.Y = matrix[MATRIX_FORWARD_Y_INDEX];
	forward.Z = matrix[MATRIX_FORWARD_Z_INDEX];
}

void MatrixHelper::GetScale(const Matrix matrix, Vector3f& scale)
{
	scale.X = matrix[MATRIX_SCALE_X_INDEX];
	scale.Y = matrix[MATRIX_SCALE_Y_INDEX];
	scale.Z = matrix[MATRIX_SCALE_Z_INDEX];
}

void MatrixHelper::GetTranslation(const Matrix matrix, Vector3f& translation)
{
	translation.X = matrix[MATRIX_TRANSLATION_X_INDEX];
	translation.Y = matrix[MATRIX_TRANSLATION_Y_INDEX];
	translation.Z = matrix[MATRIX_TRANSLATION_Z_INDEX];
}

void MatrixHelper::SetUp(Matrix matrix, const Vector3f& up)
{
	matrix[MATRIX_UP_X_INDEX] = up.X;
	matrix[MATRIX_UP_Y_INDEX] = up.Y;
	matrix[MATRIX_UP_Z_INDEX] = up.Z;
}

void MatrixHelper::SetLeft(Matrix matrix, const Vector3f& left)
{
	matrix[MATRIX_RIGHT_X_INDEX] = left.X;
	matrix[MATRIX_RIGHT_Y_INDEX] = left.Y;
	matrix[MATRIX_RIGHT_Z_INDEX] = left.Z;
}

void MatrixHelper::SetForward(Matrix matrix, const Vector3f& forward)
{
	matrix[MATRIX_FORWARD_X_INDEX] = forward.X;
	matrix[MATRIX_FORWARD_Y_INDEX] = forward.Y;
	matrix[MATRIX_FORWARD_Z_INDEX] = forward.Z;
}

void MatrixHelper::SetScale(Matrix matrix, const Vector3f& scale)
{
	matrix[MATRIX_SCALE_X_INDEX] = scale.X;
	matrix[MATRIX_SCALE_Y_INDEX] = scale.Y;
	matrix[MATRIX_SCALE_Z_INDEX] = scale.Z;
}

void MatrixHelper::SetTranslation(Matrix matrix, const Vector3f& translation)
{
	matrix[MATRIX_TRANSLATION_X_INDEX] = translation.X;
	matrix[MATRIX_TRANSLATION_Y_INDEX] = translation.Y;
	matrix[MATRIX_TRANSLATION_Z_INDEX] = translation.Z;
}

void MatrixHelper::SetRotation(Matrix matrix, const Quaternion& rotation)
{
	float d = (rotation.X * rotation.X) + (rotation.Y) * (rotation.Y) + (rotation.Z * rotation.Z) + (rotation.W * rotation.W);
    float s = 2.0f / d;
	float xs = rotation.X * s,   ys = rotation.Y * s,   zs = rotation.Z * s;
	float wx = rotation.W * xs,  wy = rotation.W * ys,  wz = rotation.W * zs;
    float xx = rotation.X * xs,  xy = rotation.X * ys,  xz = rotation.X * zs;
	float yy = rotation.Y * ys,  yz = rotation.Y * zs,  zz = rotation.Z * zs;
   
    matrix[MATRIX_RIGHT_X_INDEX] = 1.0f - (yy + zz);
    matrix[MATRIX_RIGHT_Y_INDEX] = xy - wz;
    matrix[MATRIX_RIGHT_Z_INDEX] = xz + wy;

    matrix[MATRIX_UP_X_INDEX] = xy + wz;
    matrix[MATRIX_UP_Y_INDEX] = 1.0f - (xx + zz);
    matrix[MATRIX_UP_Z_INDEX] = yz - wx;

    matrix[MATRIX_FORWARD_X_INDEX] = xz - wy;
    matrix[MATRIX_FORWARD_Y_INDEX] = yz + wx;
    matrix[MATRIX_FORWARD_Z_INDEX] = 1.0f - (xx + yy);
}